# -*- encoding: UTF-8 -*-
#
# This is a tiny example that shows how to show live images from Nao using PyQt.
# You must have python-qt4 installed on your system.
#

import sys

from PyQt4.QtGui import QWidget, QImage, QApplication, QPainter
from naoqi import ALProxy
import cv2
import numpy as np

# To get the constants relative to the video.
import vision_definitions

a = False

class ImageWidget(QWidget):

    """
    Tiny widget to display camera images from Naoqi.
    """
    def __init__(self, IP, PORT, CameraID, parent=None):
        """
        Initialization.
        """
        QWidget.__init__(self, parent)
        self._image = QImage()
        self.setWindowTitle('Nao')


        self._imgWidth = 320
        self._imgHeight = 240
        self._cameraID = CameraID
        self.resize(self._imgWidth, self._imgHeight)

        # Proxy to ALVideoDevice.
        self._videoProxy = None

        # Our video module name.
        self._imgClient = ""

        # This will contain this alImage we get from Nao.
        self._alImage = None

        self._registerImageClient(IP, PORT)

        # Trigget 'timerEvent' every 100 ms.

        self.startTimer(10)



    def _registerImageClient(self, IP, PORT):
        """
        Register our video module to the robot.
        """
        self._videoProxy = ALProxy("ALVideoDevice", IP, PORT)
        resolution = vision_definitions.kQVGA  # 320 * 240
        colorSpace = vision_definitions.kRGBColorSpace
        self._imgClient = self._videoProxy.subscribe("_client", resolution, colorSpace, 5)

        # Select camera.
        self._videoProxy.setParam(vision_definitions.kCameraSelectID,
                                  self._cameraID)


    def _unregisterImageClient(self):
        """
        Unregister our naoqi video module.
        """
        if self._imgClient != "":
            self.releaseShortcut(self._imgClient)
            self._videoProxy.unsubscribe(self._imgClient)


    def paintEvent(self, event):
        """
        Draw the QImage on screen.
        """
        painter = QPainter(self)
        painter.drawImage(painter.viewport(), self._image)


    def _updateImage(self):
        """
        Retrieve a new image from Nao.
        """
        # self._unregisterImageClient()
        # sys.exit(app.exec_())
        # sys.exit(app.exec_())

        self._alImage = self._videoProxy.getImageRemote(self._imgClient)
        self._image = QImage(self._alImage[6],  # Pixel array.
                             self._alImage[0],  # Width.
                              self._alImage[1],  # Height.
                             QImage.Format_RGB888)

        ptr = self._image.constBits()
        ptr.setsize(self._image.byteCount())
        # mat = np.array(ptr).reshape(self._image.height(), self._image.width(), 4)  # 注意这地方通道数一定要填4，否则出错
        mat = np.array(ptr).reshape(240, 240, 4)

        template = cv2.imread("jiantou15.jpg", cv2.IMREAD_GRAYSCALE)

        w, h = template.shape[::-1]
        gray_frame = cv2.cvtColor(mat, cv2.COLOR_BGR2GRAY)
        print(gray_frame.shape)
        res = cv2.matchTemplate(gray_frame, template, cv2.TM_CCOEFF_NORMED)
        loc = np.where(res >= 0.7)
        for pt in zip(*loc[::-1]):
            cv2.rectangle(mat, pt, (pt[0] + w, pt[1] + h), (0, 255, 0), 3)
            global a
            #if pt[1] + h:
            print ("SBDFX")
            a = True
            print("a",a)
                # sys.exit(app.exec_())






        self._Image = cv2.cvtColor(mat, cv2.COLOR_BGR2RGB)  # 将BGR转为RGB
        cv2.imshow("Frame", mat)


        #return self._image

    def timerEvent(self, event):
        """
        Called periodically. Retrieve a nao image, and update the widget.
        """
        self._updateImage()
        self.update()


    def __del__(self):

        self._unregisterImageClient()




if __name__ == '__main__':
    IP = "192.168.1.7"  # Replace here with your NaoQi's IP address.
    PORT = 9559
    CameraID = 0

    # Read IP address from first argument if any.
    if len(sys.argv) > 1:
        IP = sys.argv[1]

    # Read CameraID from second argument if any.
    if len(sys.argv) > 2:
        CameraID = int(sys.argv[2])


    app = QApplication(sys.argv)
    myWidget = ImageWidget(IP, PORT, CameraID)
    myWidget.show()
    sys.exit(app.exec_())



    # template = cv2.imread("jiantou.jpg.jpg", cv2.IMREAD_GRAYSCALE)
    # w, h = template.shape[::-1]
    # while True:
    #
    #     gray_frame = cv2.cvtColor(self._image, cv2.COLOR_BGR2GRAY)
    #     res = cv2.matchTemplate(gray_frame, template, cv2.TM_CCOEFF_NORMED)
    #     loc = np.where(res >= 0.7)
    #     for pt in zip(*loc[::-1]):
    #         cv2.rectangle(frame, pt, (pt[0] + w, pt[1] + h), (0, 255, 0), 3)
    #     cv2.imshow("Frame", frame)
    #     key = cv2.waitKey(1)
    #     if key == 27:
    #         break
    # cap.release()
    # cv2.destroyAllWindows()


